/**
 * @file sfifo.c
 * @author xy
 * @brief 
 * @version 0.1
 * @date 2024-12-28
 * 
 * @copyright Copyright (c) 2024
 * 
 */

#include "sfifo.h"

unsigned int sfifo_mask(unsigned int bsize)
{
    unsigned int masktmp = 1;
    unsigned int i = 0;
    for (i = 0; i < 32 - 1; i++)
    {
        if ((masktmp << 1) > bsize)
        {
            break;
        } else
        {
            masktmp <<= 1;
        }
    }
    return masktmp - 1;
}

void sfifo_memcpy(void *dst, const void *src, unsigned int len)
{
    unsigned char *d = dst;
    const unsigned char *s = src;
    while (len--)
    {
        *d++ = *s++;
    }
}

static inline unsigned int sfifo_space(const struct sfifo_s *fifo)
{
    return (fifo->mask + 1) - (fifo->in - fifo->out);
}


int sfifo_init(struct sfifo_s *fifo, void *buf, unsigned int size,
               unsigned int esize)
{
    fifo->data = buf;
    fifo->mask = sfifo_mask(size / esize);
    fifo->esize = esize;
    fifo->in = 0;
    fifo->out = 0;
    return 0;
}

static inline unsigned int sfifo_min(unsigned int a, unsigned int b)
{
    return a < b ? a : b;
}

static void sfifo_copy_in(const struct sfifo_s *fifo, const void *src,
                          unsigned int len, unsigned int off)
{
    unsigned int size_tmp = fifo->mask + 1;
    const unsigned int esize_tmp = fifo->esize;
    unsigned int off_tmp = off & fifo->mask;
    unsigned int divid = 0;
    if (esize_tmp != 1)
    {
        off_tmp *= esize_tmp;
        size_tmp *= esize_tmp;
        len *= esize_tmp;
    }
    divid = sfifo_min(len, size_tmp - off_tmp);
    sfifo_memcpy((unsigned char *)fifo->data + off_tmp, src, divid);
    sfifo_memcpy(fifo->data, (unsigned char *)src + divid, len - divid);
}

unsigned int sfifo_in(struct sfifo_s *fifo, const void *buf, unsigned int len)
{
    unsigned int spacetmp = sfifo_space(fifo);
    if (spacetmp > (fifo->mask + 1))
    {
        spacetmp = 0;
        fifo->in = fifo->out;
    }
    if (len > spacetmp)
    {
        len = spacetmp;
    }
    sfifo_copy_in(fifo, buf, len, fifo->in);
    fifo->in += len;
    return len;
}
static void sfifo_copy_out(const struct sfifo_s *fifo, void *dst,
                            unsigned int len, unsigned int off)
{
    unsigned int size_tmp = fifo->mask + 1;
    unsigned int esize_tmp = fifo->esize;
    unsigned int off_tmp = off & fifo->mask;
    unsigned int divid = 0;
    if (esize_tmp != 1)
    {
        off_tmp *= esize_tmp;
        size_tmp *= esize_tmp;
        len *= esize_tmp;
    }
    divid = sfifo_min(len, size_tmp - off_tmp);
    sfifo_memcpy(dst, (unsigned char *)fifo->data + off_tmp, divid);
    sfifo_memcpy((unsigned char *)dst + divid, fifo->data, len - divid);
}

unsigned int sfifo_out(struct sfifo_s *fifo, void *buf, unsigned int len)
{
    unsigned int datalen = fifo->in - fifo->out;
    if (datalen > (fifo->mask + 1))
    {
        datalen = 0;
        fifo->out = fifo->in;
    }
    if (len > datalen)
    {
        len = datalen;
    }
    sfifo_copy_out(fifo, buf, len, fifo->out);
    fifo->out += len;
    return len;
}

void sfifo_clear_in(struct sfifo_s *fifo)
{
    fifo->in = fifo->out;
}

void sfifo_clear_out(struct sfifo_s *fifo)
{
    fifo->out = fifo->in;
}


unsigned int sfifo_used(struct sfifo_s *fifo)
{
    unsigned int datalen = fifo->in - fifo->out;
    if (datalen > (fifo->mask + 1))
    {
        datalen = 0;
        fifo->out = fifo->in;
    }
    return datalen;
}

unsigned int sfifo_free(struct sfifo_s *fifo)
{
    return sfifo_space(fifo);
}
